#include "BPhysicsEngine.h"
#include "Engine.h"
#include "ResourceManager.h"
#include "BPhysicsRayCallBack.h"
#include "SeekBehaviour.h"
#include "FleeBehaviour.h"
#include "ArriveBehaviour.h"
#include "WanderBehaviour.h"
#include "FormationBehaviour.h"
#include "AvoidanceBehaviour.h"
#include "WallAvoidanceBehaviour.h"
#include "DirectXMesh.h"
#include "PathFollowBehaviour.h"
#include "Input.h"
#include "GameState.h"


BPhysicsEngine::BPhysicsEngine() : 
m_dynamicsWorld(NULL),
m_broadphase(NULL),
m_dispatcher(NULL),
m_solver(NULL),
m_collisionConfiguration(NULL),
m_debugDrawer(NULL),
m_initialised(false)
{}
void BPhysicsEngine::Update(float dt)
{
    ///step the simulation
    if (m_dynamicsWorld)
    {
        m_dynamicsWorld->stepSimulation((float)dt); //compiler converts to long, and then long to float,
    }
    for(int i = 0; i < m_actors.size(); ++i)
    {
        if(m_actors[i]->IsAwake())
            m_actors[i]->Update((float)dt);
    }
}
void BPhysicsEngine::Initialize(const D3DXVECTOR3& Gravity)
{
    Release();
    ///collision configuration contains default setup for memory, collision setup
    m_collisionConfiguration = new btDefaultCollisionConfiguration();
    if(!m_collisionConfiguration)
        assert(0);
    //m_collisionConfiguration->setConvexConvexMultipointIterations();

    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
    m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
    if(!m_dispatcher)
        assert(0);
    //not the default, i like the sound of this one haha
    m_broadphase = new btDbvtBroadphase();
    if(!m_broadphase)
        assert(0);
    ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
    btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
    if(!sol)
        assert(0);
    m_solver = sol;

    m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
    btVector3 grav(Gravity.x, Gravity.y, Gravity.z);
    m_dynamicsWorld->setGravity(grav);

    m_debugDrawer = Engine::Instance()->GetLineDrawer();
    m_dynamicsWorld->setDebugDrawer(m_debugDrawer);
    m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
    btGImpactCollisionAlgorithm::registerAlgorithm(m_dispatcher);

    m_initialised = true;
}
void BPhysicsEngine::Release()
{
    //As the actors were the last thing to be created, we should destroy them first
    for(int i = 0; i < m_actors.size(); ++ i)
    {
        if(!m_actors[i]->IsAwake())
            AddActor(m_actors[i]);
        m_actors[i]->CleanUp();
        delete m_actors[i];
    }
    m_actors.clear();
    m_actorMap.clear();
    //from now on, its chaos!
    if(m_dynamicsWorld)
    {
        //Ok, starting at the end of the array, and working back to 0
        //this avoids re sizing
        for (int x = m_dynamicsWorld->getNumCollisionObjects() - 1; x >= 0; x--)
        {
            //get the last object in the array,
            btCollisionObject * obj = m_dynamicsWorld->getCollisionObjectArray()[x];
            if (!obj) 
                continue; 
            //if this is a rigid body( It should be!...this wmay leak memory if not! )
            if (obj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
            {
                //This is safe (I HOPE) To do.
                btRigidBody * body = btRigidBody::upcast(obj);
                if (body)
                {
                    //Ok, this was leaking memory if shapes were colliding when we shut down
                    if ( m_broadphase && m_dispatcher )
                    {
                        if ( m_broadphase->getOverlappingPairCache() )
                        {   //if our broadphase has a valid cache, we must clean this body from it
                            m_broadphase->getOverlappingPairCache()->cleanProxyFromPairs(
                                body->getBroadphaseHandle(),
                                m_dispatcher);
                        }
                    }
                }
                //if our body has a valid motion state, delete it here
                //This could be managed in my Actor class...
                if (body)
                {
                    if (body->getMotionState()) delete body->getMotionState();
                }
            }
            //finally, remove the object from the world, and delete it
            m_dynamicsWorld->removeCollisionObject(obj);
            delete obj;
            obj = NULL;
        }
    }

    //if our dispatcher is valid...
    if(m_dispatcher)
    {
        for ( int i = 0; i < m_dispatcher->getNumManifolds(); i++ )
        {
            //clear any manifolds
            btPersistentManifold * manifold = m_dispatcher->getManifoldByIndexInternal(i);
            manifold->clearManifold();
        }
    }

    //ok, this is the annoying part
    for (int x = 0; x < m_collisionShapes.size(); x++)
    {
        //first, check the shape is valid
        btCollisionShape * shape = m_collisionShapes[x];
        if (!shape) 
            continue;
        //if we are a compound shape, we are made of lots of different shapes...
        if ( shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE )
        {
            //so we need to iterate over all the child shapes to delete them
            btCompoundShape * compound = (btCompoundShape *) shape;
            for ( int z = 0; z < compound->getNumChildShapes(); z++ )
            {
                btCollisionShape * child = (btCollisionShape *) compound->getChildShape(z);
                if (!child) 
                    continue;
                compound->removeChildShape(child);
                delete child;
                child = NULL;
            }
        }
        delete shape;//finally, delete the shape
        shape = NULL;
    }
    //not necessary, but tidy
    m_collisionShapes.clear();

    //Now we just safely delete all the member variables
    if (m_collisionConfiguration)
    {
        delete m_collisionConfiguration;
        m_collisionConfiguration = NULL;
    }

    if (m_dispatcher)
    {
        delete m_dispatcher;
        m_dispatcher = NULL;
    }

    if (m_broadphase)
    {
        delete m_broadphase;
        m_broadphase = NULL;
    }

    if (m_solver)
    {
        delete m_solver;
        m_solver = NULL;
    }

    if (m_dynamicsWorld)
    {
        delete m_dynamicsWorld;
        m_dynamicsWorld = NULL;
    }

    m_initialised = false;
}
void BPhysicsEngine::FireBox(int whichModel)
{
    BPhysicsActor* actor;
    if(whichModel == 0)
    {
        actor = CreateBox();
    }
    else
    {
        switch(whichModel)
        {
        case 1:
            {
                actor = new BPhysicsActor(1.0f, btVector3(20,20,20));
                actor->CreateFromMesh(GET_MESH("bunny_physics"),false);
                actor->SetMesh(GET_MESH("bunny"));
                actor->CreatePerceptionOccluder(Perception::TARGET_1, GET_MESH("bunny_physics"));
                break;
            }
        case 2:
            {
                actor = new BPhysicsActor(1.0f, btVector3(1,1,1));
                actor->CreateFromMesh(GET_MESH("teapot_physics"),false);
                actor->SetMesh(GET_MESH("teapot"));
                actor->CreatePerceptionOccluder(Perception::TARGET_2, GET_MESH("teapot_physics"));
                break;
            }
        }
        AddActor(actor);
        actor->SetFriction(1.0f);
        actor->SetMaxVelocity(30.0f);
    }
    Vec3 cPos = Engine::Instance()->GetCurrentState()->GetCamera()->GetPos();
    btVector3 spawnPos(cPos.X(), cPos.Y(), cPos.Z());
    actor->SetPosition(spawnPos);

    btVector3 dir = GetClickDirection();
    dir *= 3000.0f;
    actor->ApplyCentralForce(dir);
}
void BPhysicsEngine::AddVehicle()
{
    BPhysicsActor* actor = CreateVehicle();
    btVector3 spawnPos(0,5,0);
    actor->SetPosition(spawnPos);
    actor->SetAiCollisionRadius(5.0f);
}
BPhysicsActor* BPhysicsEngine::CreateBox()
{
    BPhysicsActor* actor = new BPhysicsActor(1.0f, btVector3(1,1,1));
    actor->CreateAsCube();  
    AddActor(actor);
    actor->SetFriction(1.0f);
    actor->SetMaxVelocity(30.0f);
    return actor;
}
BPhysicsActor* BPhysicsEngine::CreateVehicle()
{
    BPhysicsActor* actor = new BPhysicsActor(50.0f, btVector3(0.3f,0.3f,0.3f));
    actor->CreateAsVehicle("car");
    actor->GetMesh()->SetTexture(GET_TEXTURE("policeCar.jpg"));
    AddActor(actor);
    actor->SetFriction(0.6f);
    actor->SetMaxVelocity(350.0f);
    return actor;
}
BPhysicsActor* BPhysicsEngine::CreateAiAgent()
{
    BPhysicsActor* actor = new BPhysicsActor(1.0f, btVector3(1,1,1));
    actor->CreateFromMesh("unitCube",false);
    AddActor(actor);
    actor->SetAiCollisionRadius(1.2f);
    btVector3 spawnPos(0,3,0);
    actor->SetThinking(true);
    actor->SetMaxVelocity(30.0f);
    actor->SetPosition(spawnPos);
    actor->SetFriction(0.6f);
    return actor;
}
void BPhysicsEngine::AddCollisionShape( btCollisionShape* shape )
{
    m_collisionShapes.push_back(shape);
}
void BPhysicsEngine::AddActor( BPhysicsActor* actor)
{
    if(actor)
    {
        if(actor->GetBodyPointer())
        {
            m_dynamicsWorld->addRigidBody(actor->GetBodyPointer());
        }
        if(actor->HasBeenAddedToMap() == 0)
        {
            if(actor->GetShapePointer())
                AddCollisionShape(actor->GetShapePointer());

            m_actorMap[actor->GetBodyPointer()] = actor;
            m_actors.push_back(actor);
            actor->AddedToMap();
        }
    }
}
vActors& BPhysicsEngine::GetActors()
{
    return m_actors;
}
void BPhysicsEngine::CleanActors()
{
    //As the actors were the last thing to be created, we should destroy them first
    for(int i = 0; i < m_actors.size(); ++ i)
    {
        if(!m_actors[i]->IsAwake())
            AddActor(m_actors[i]);
        m_actors[i]->CleanUp();
        delete m_actors[i];
    }
    m_actors.clear();
    //from now on, its chaos!
    if(m_dynamicsWorld)
    {
        //Ok, starting at the end of the array, and working back to 0
        //this avoids re sizing
        for (int x = m_dynamicsWorld->getNumCollisionObjects() - 1; x >= 0; x--)
        {
            //get the last object in the array,
            btCollisionObject * obj = m_dynamicsWorld->getCollisionObjectArray()[x];
            if (!obj) 
                continue; 
            //if this is a rigid body( It should be!...this wmay leak memory if not! )
            if (obj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
            {
                //This is safe (I HOPE) To do.
                btRigidBody * body = btRigidBody::upcast(obj);
                if (body)
                {
                    //Ok, this was leaking memory if shapes were colliding when we shut down
                    if ( m_broadphase && m_dispatcher )
                    {
                        if ( m_broadphase->getOverlappingPairCache() )
                        {   //if our broadphase has a valid cache, we must clean this body from it
                            m_broadphase->getOverlappingPairCache()->cleanProxyFromPairs(
                                body->getBroadphaseHandle(),
                                m_dispatcher);
                        }
                    }
                }
                //if our body has a valid motion state, delete it here
                //This could be managed in my Actor class...
                if (body)
                {
                    if (body->getMotionState()) delete body->getMotionState();
                }
            }
            //finally, remove the object from the world, and delete it
            m_dynamicsWorld->removeCollisionObject(obj);
            delete obj;
            obj = NULL;
        }
    }

    //if our dispatcher is valid...
    if(m_dispatcher)
    {
        for ( int i = 0; i < m_dispatcher->getNumManifolds(); i++ )
        {
            //clear any manifolds
            btPersistentManifold * manifold = m_dispatcher->getManifoldByIndexInternal(i);
            manifold->clearManifold();
        }
    }

    //ok, this is the annoying part
    for (int x = 0; x < m_collisionShapes.size(); x++)
    {
        //first, check the shape is valid
        btCollisionShape * shape = m_collisionShapes[x];
        if (!shape) 
            continue;
        //if we are a compound shape, we are made of lots of different shapes...
        if ( shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE )
        {
            //so we need to iterate over all the child shapes to delete them
            btCompoundShape * compound = (btCompoundShape *) shape;
            for ( int z = 0; z < compound->getNumChildShapes(); z++ )
            {
                btCollisionShape * child = (btCollisionShape *) compound->getChildShape(z);
                if (!child) 
                    continue;
                compound->removeChildShape(child);
                delete child;
                child = NULL;
            }
        }
        delete shape;//finally, delete the shape
        shape = NULL;
    }
    //not necessary, but tidy
    m_collisionShapes.clear();
}
void BPhysicsEngine::RemoveVehicle( btRaycastVehicle* v )
{
    if (!v)    
        return;
    btRigidBody * body = v->getRigidBody();
    if (!body) 
        return;
    m_broadphase->getOverlappingPairCache()->cleanProxyFromPairs(
        body->getBroadphaseHandle(),
        m_dispatcher);
    m_dynamicsWorld->removeVehicle(v);
}
void BPhysicsEngine::SetDebugDrawMode( int param1 )
{
    if(param1 == 0)
        m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
    else if(param1 == 1)
        m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawAabb);
}
void BPhysicsEngine::CallUpdateGrid()
{
    for(int i = 0; i < m_actors.size(); ++i)
    {
        m_actors[i]->UpdateGrid();
    }
}
/* Called On Right click: works out (roughly) where the click is taking place on the rigidbody,
   and applies a force scaled by param1 in that direction*/
void BPhysicsEngine::ApplyPushForce( float param1 )
{
    Vec3 pos = Engine::Instance()->GetCurrentState()->GetCamera()->GetPos();
    btVector3 from(pos.X(), pos.Y(), pos.Z());
    btVector3 dir = GetClickDirection();
    btVector3 to = from + dir * 5000;
    //create my own callback that stores the triangle index that has been hit
    MyRayCallBack cb(from, to);
    if(m_dynamicsWorld)
        m_dynamicsWorld->rayTest(from,to,cb);
    if(cb.hasHit())
    {
        btCollisionObject * object = cb.m_collisionObject;
        if (!object) 
            return;

        //I don't know of any other way to do this?!
        btRigidBody* body = btRigidBody::upcast(object);
        ActorMap::iterator it = m_actorMap.find(body);
        if(it != m_actorMap.end())
        {
            BPhysicsActor* actor = it->second;
            actor->SetActive(true);
            btTransform t = actor->GetBodyPointer()->getWorldTransform().inverse();
            btVector3 hitPointLocal = t * cb.m_hitPointWorld;
            actor->ApplyForceAtPoint(dir * param1,hitPointLocal);
        }
    }
}
/* Taken from Callum Brightings Real Time Ray Tracer */
btVector3 BPhysicsEngine::GetClickDirection()
{
    int xP = (int)Input::Instance()->GetMouse().GetPosition().x;
    int yP = (int)Input::Instance()->GetMouse().GetPosition().y;
    
    int screenWidth = Engine::Instance()->GetScreenRes(0);
    int screenHeight = Engine::Instance()->GetScreenRes(1);

    const D3DXMATRIX pmatProj = Engine::Instance()->GetCurrentState()->GetCamera()->GetProjectionMatrix();

    // Compute the vector of the pick ray in screen space
    D3DXVECTOR3 v;
    v.x = ( ( ( 2.0f * xP ) / screenWidth ) - 1 ) / pmatProj._11;
    v.y = -( ( ( 2.0f * yP ) / screenHeight ) - 1 ) / pmatProj._22;
    v.z = 1.0f;

    // Get the inverse view matrix
    D3DXMATRIX mView = Engine::Instance()->GetCurrentState()->GetCamera()->GetViewMatrix();
    D3DXMATRIX inv;
    D3DXMatrixIdentity(&inv);
    D3DXMatrixInverse( &inv, NULL, &mView );

    // Transform the screen space pick ray into 3D space
    D3DXVECTOR3 rayDir;
    rayDir.x = v.x * inv._11 + v.y * inv._21 + v.z * inv._31;
    rayDir.y = v.x * inv._12 + v.y * inv._22 + v.z * inv._32;
    rayDir.z = v.x * inv._13 + v.y * inv._23 + v.z * inv._33;
    return btVector3(rayDir.x,rayDir.y,rayDir.z).normalized();
}

/* I want this here, not called elsewhere throughout the code */
void BPhysicsEngine::DisableBody(BPhysicsActor* actor)
{
    if(!actor->IsAwake())
        return;
    actor->SetAwake(false);
    if(m_dynamicsWorld)
    {
        btCollisionObject* actorOB = actor->GetBodyPointer();
        //Ok, starting at the end of the array, and working back to 0
        //this avoids re sizing
        for (int x = m_dynamicsWorld->getNumCollisionObjects() - 1; x >= 0; x--)
        {
            //get the last object in the array,
            btCollisionObject * obj = m_dynamicsWorld->getCollisionObjectArray()[x];
            if (!obj) 
                continue; 

            if(actorOB == obj)
            {
                //if this is a rigid body( It should be!...this wmay leak memory if not! )
                if (obj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                {
                    //This is safe (I HOPE) To do.
                    btRigidBody * body = btRigidBody::upcast(obj);
                    if (body)
                    {
                        //Ok, this was leaking memory if shapes were colliding when we shut down
                        if ( m_broadphase && m_dispatcher )
                        {
                            if ( m_broadphase->getOverlappingPairCache() )
                            {   //if our broadphase has a valid cache, we must clean this body from it
                                m_broadphase->getOverlappingPairCache()->cleanProxyFromPairs(
                                    body->getBroadphaseHandle(),
                                    m_dispatcher);
                            }
                        }
                    }
                }            
                //finally, remove the object from the world, and delete it
                m_dynamicsWorld->removeCollisionObject(obj);
            }

        }
    }

}
void BPhysicsEngine::EnableBody(BPhysicsActor* actor)
{
    if(actor->IsAwake())
        return;
    actor->SetAwake(true);
    AddActor(actor);
}
BPhysicsActor* BPhysicsEngine::PickActor()
{
    Vec3 pos = Engine::Instance()->GetCurrentState()->GetCamera()->GetPos();
    btVector3 from(pos.X(), pos.Y(), pos.Z());
    btVector3 dir = GetClickDirection();
    btVector3 to = from + dir * 5000;
    BPhysicsActor* actor = NULL;
    //create my own callback that stores the triangle index that has been hit
    MyRayCallBack cb(from, to);
    if(m_dynamicsWorld)
        m_dynamicsWorld->rayTest(from,to,cb);
    if(cb.hasHit())
    {
        btCollisionObject * object = cb.m_collisionObject;
        if (!object) 
            return actor;

        //I don't know of any other way to do this?!
        btRigidBody* body = btRigidBody::upcast(object);
        ActorMap::iterator it = m_actorMap.find(body);
        if(it != m_actorMap.end())
        {
            Engine::Instance()->GetCurrentState()->HandleMessage(it->second->GetMesh()->GetFileName().c_str());
            if(it->second->GetType() == BPhysicsActor::Actor_Dynamic)
                actor = it->second;
        }
    }
    return actor;
}
void BPhysicsEngine::DrawDebug()
{
    m_dynamicsWorld->debugDrawWorld();
}

bool BPhysicsEngine::RayTest( const btVector3& rayFrom,const btVector3& rayTo, btCollisionWorld::ClosestRayResultCallback& rayCallback )
{
    if(m_dynamicsWorld)
    {
        m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
        return rayCallback.hasHit();
    }
    else 
        return false;
}

BPhysicsActor* BPhysicsEngine::SearchForActor(btRigidBody* body)
{
    BPhysicsActor* actor = NULL;
    ActorMap::iterator it = m_actorMap.find(body);
    if(it != m_actorMap.end())
    {
        Engine::Instance()->GetCurrentState()->HandleMessage(it->second->GetMesh()->GetFileName().c_str());
        if(it->second->GetType() == BPhysicsActor::Actor_Dynamic)
            actor = it->second;
    }
    return actor;
}
